#pragma once

#include "Matrix.hpp"

// ====================================================

struct Solver
{
    // 矩阵操作
    static Matrix<Real> inverse(Matrix<Real> &A);
    static Real Householder(Vector<Real> &x, Vector<Real> &v);
    static Vector<Real> Givens(Vector<Real> &v, int i = 0, int j = 1);

    // 矩阵分解
    static bool LU(Matrix<Real> &A);                                                    // LU 分解
    static void LU(Matrix<Real> &A, Perm &perm);                                        // 列主元 LU 分解
    static bool Cholesky(Matrix<Real> &A);                                              // Cholesky
    static bool Cholesky_D(Matrix<Real> &A);                                            // 优化的 Cholesky
    static void QR(Matrix<Real> &A, Vector<Real> &d);                                   // QR 分解
    static void rotate(Matrix<Real> &A, Matrix<Real> &Q, int p, int q);                 // Jacobi 平面旋转
    static void HAH(Matrix<Real> &A, int sR, int eR, int sC, int eC, Vector<Real> &v);  // 标准 HAH ,可选累积正交变换
    static void HAH(Matrix<Real> &A, int sR, int eR, Vector<Real> &v);                  // 对称形式的 HAH ,可选累积正交变换
    static void Hessenberg(Matrix<Real> &A, Matrix<Real> &Q, bool ifSymmetric = false); // 上 Hessenberg 化
    static void Real_QR(Matrix<Real> &A, int l, int m);                                 // 对指定区域的双重步 QR 迭代法
    static void Wilkinson_QR(Matrix<Real> &A, int l, int m);                            // 使用 Wilkinson 位移的对称 QR 迭代

    // 常规解法
    static Vector<Real> solve_upper(Matrix<Real> &A, Vector<Real> &b, bool unit = false); // 是否按照单位上三角阵求解
    static Vector<Real> solve_lower(Matrix<Real> &A, Vector<Real> &b, bool unit = false); // 是否按照单位下三角阵求解
    static Vector<Real> solve(Matrix<Real> &A, Vector<Real> &b, bool choose = false);     // 是否选主元求解
    static Vector<Real> cholesky(Matrix<Real> &A, Vector<Real> &b, bool improve = false); // 是否优化为 LDL 分解

    // 最小二乘问题
    static Vector<Real> qr(Matrix<Real> &A, Vector<Real> &b); // QR 分解法
    static Vector<Real> cg(Matrix<Real> &A, Vector<Real> &b); // 共轭梯度法

    // 迭代法
    static Vector<Real> Jacobi(Matrix<Real> &A, Vector<Real> &b);
    static Vector<Real> G_S(Matrix<Real> &A, Vector<Real> &b);
    static Vector<Real> SOR(Matrix<Real> &A, Vector<Real> &b, Real w);

    // 条件数
    static Real kappa(Matrix<Real> &A);                                               // 盲人爬山法
    static Real caculate_analysis(Matrix<Real> &A, Vector<Real> &x, Vector<Real> &b); // 线性方程的计算误差界

    // 特征值与特征向量
    static Real power(Matrix<Real> &A, Real mu = 0);                   // 幂法
    static Vector<Real> inverse_power(Matrix<Real> &A, Real lambda);   // 反幂法（半次迭代法）
    static int implicit_qr(Matrix<Real> &A, bool ifSymmetric = false); // 隐式 QR 方法
    static Matrix<Real> pass_Jacobi(Matrix<Real> &A, Real sigma);      // 过关 Jacobi 方法
    static Vector<Real> bisection(Matrix<Real> &A);                    // 二分法
    // static bool eigen(Matrix<Real> &A, Matrix<Complex> &eigenM, Vector<Real> &eigenV); // 应用隐式 QR 方法和改进的反幂法求出特征值和特征向量
};

// =====================================================

Matrix<Real> Solver::inverse(Matrix<Real> &A)
{
    int row = A.row();
    Matrix<Real> m(Matrix<Real>::unit(row));
    if (!A.square())
    {
        return m;
    }
    Matrix<Real> old(A);

    Real max;  // 最大值
    int index; // 最大值位置
    for (int i = 0; i < row; i++)
    {
        // find max
        max = 0;
        index = i;
        for (int j = i; j < row; j++)
        {
            Real tmp = fabs(old(j, i));
            if (tmp > max)
            {
                max = tmp;
                index = j;
            }
        }
        if (max == 0)
        {
            return m;
        }
        // change max
        for (int j = 0; j < row; j++)
        {
            Real tmp = old(i, j);
            old(i, j) = old(index, j);
            old(index, j) = tmp;

            tmp = m(i, j);
            m(i, j) = m(index, j);
            m(index, j) = tmp;
        }
        // start
        for (int j = 0; j < row; j++)
        {
            Real tmp = old(i, i);
            if (i == j)
            {
                for (int k = 0; k < row; k++)
                {
                    m(j, k) /= tmp;
                    old(j, k) /= tmp;
                }
                continue;
            }
            tmp = old(j, i) / old(i, i);
            for (int k = 0; k < row; k++)
            {
                m(j, k) -= tmp * m(i, k);
                old(j, k) -= tmp * old(i, k);
            }
        }
    }
    return m;
}

Real Solver::Householder(Vector<Real> &x, Vector<Real> &v)
{
    v = Vec::normalize(x, 0); // 防止溢出
    Real sumOfx = Vec::dot(v, v);
    Real sumOfx1 = sumOfx - v[0] * v[0];

    Real beta;
    if (sumOfx1 == 0)
    {
        beta = 0;
    }
    else
    {
        if (x[0] <= 0)
        {
            v[0] -= sqrt(sumOfx);
        }
        // 如果是正的分量，则用特殊算法减小舍入误差
        else
        {
            v[0] = -1 * sumOfx1 / (v[0] + sqrt(sumOfx));
        }
        // 对 beta 乘 v[0] * v[0] ，抵消规格化为 1 的影响
        beta = 2 * v[0] * v[0] / (sumOfx1 + v[0] * v[0]);
        v = v / v[0];
    }
    return beta;
}

Vector<Real> Solver::Givens(Vector<Real> &v, int i, int j)
{
    Real sqrt_x = sqrt(v[i] * v[i] + v[j] * v[j]);
    if (sqrt_x == 0)
    {
        return Vector<Real>::unit(2);
    }
    return Vector<Real>({v[i] / sqrt_x, v[j] / sqrt_x});
}

bool Solver::LU(Matrix<Real> &A)
{
    if (!A.square())
    {
        return false;
    }
    int n = A.row();
    for (int i = 0; i < n; i++)
    {
        Real key = A(i, i);
        // 无法进行高斯变换
        if (key == 0)
        {
            return false;
        }
        // 第 i 列高斯变换
        for (int j = i + 1; j < n; j++)
        {
            A(j, i) /= key;
            for (int k = i + 1; k < n; k++)
            {
                A(j, k) -= A(i, k) * A(j, i);
            }
        }
    }
    return true;
}

// 返回置换向量
void Solver::LU(Matrix<Real> &A, Perm &perm)
{
    if (!A.square())
    {
        return;
    }
    int n = A.row();
    Real max, tmp; // 记录最大值
    int index;     // 记录要交换的主元位置
    for (int i = 0; i < n; i++)
    {
        max = 0;
        index = i;
        // 选取列主元
        for (int p = i; p < n; p++)
        {
            tmp = fabs(A(p, i));
            if (tmp > max)
            {
                max = tmp;
                index = p;
            }
        }
        // 在这里奇异,说明 A 的秩为 i
        if (max == 0)
        {
            return;
        }
        // 交换主元
        for (int q = 0; q < n; q++)
        {
            tmp = A(i, q);
            A(i, q) = A(index, q);
            A(index, q) = tmp;
        }
        perm.exchange(i, index);
        // 正常的高斯消去法
        for (int j = i + 1; j < n; j++)
        {
            A(j, i) /= A(i, i);
            for (int k = i + 1; k < n; k++)
            {
                A(j, k) -= A(i, k) * A(j, i);
            }
        }
    }
}

// LL
bool Solver::Cholesky(Matrix<Real> &A)
{
    if (!A.square())
    {
        return false;
    }
    int n = A.row();
    Real sum = 0;
    for (int i = 0; i < n; i++)
    {
        sum = 0;
        for (int p = 0; p < i; p++)
        {
            sum += A(i, p) * A(i, p);
        }
        A(i, i) = sqrt(A(i, i) - sum);
        if (A(i, i) == 0)
        {
            return false;
        }
        for (int j = i + 1; j < n; j++)
        {
            sum = 0;
            for (int k = 0; k < i; k++)
            {
                sum += A(j, k) * A(i, k);
            }
            A(j, i) = (A(j, i) - sum) / A(i, i);
            A(i, j) = A(j, i);
        }
    }
    return true;
}

// LDL
bool Solver::Cholesky_D(Matrix<Real> &A)
{
    if (!A.square())
    {
        return false;
    }
    int n = A.row();
    Real sum = 0;
    for (int i = 0; i < n; i++)
    {
        sum = 0;
        for (int p = 0; p < i; p++)
        {
            sum += A(i, p) * A(i, p) * A(p, p);
        }
        A(i, i) -= sum;
        if (A(i, i) == 0)
        {
            return false;
        }
        for (int j = i + 1; j < n; j++)
        {
            sum = 0;
            for (int k = 0; k < i; k++)
            {
                sum += A(j, k) * A(i, k) * A(k, k);
            }
            A(j, i) -= sum;
            A(j, i) /= A(i, i);
            A(i, j) = A(j, i);
        }
    }
    return true;
}

void Solver::QR(Matrix<Real> &A, Vector<Real> &d)
{
    int column = A.col(), row = A.row();
    for (int i = 0; i < column; i++)
    {
        Vector<Real> x(row - i);
        Vector<Real> v(row - i);
        Real beta;
        for (int j = i; j < row; j++)
        {
            x[j - i] = A(j, i);
        }
        beta = Householder(x, v);
        Vector<Real> w(column - i);
        // get w = beta * AT * v
        for (int k = i; k < column; k++)
        {
            Real sum = 0;
            for (int p = i; p < row; p++)
            {
                sum += A(p, k) * v[p - i];
            }
            // note: it is w[k-i], not w[k]
            w[k - i] = beta * sum;
        }
        // get HA = A - v * wT
        for (int k = i; k < row; k++)
        {
            for (int p = i; p < column; p++)
            {
                if (p == i && k > i)
                {
                    A(k, p) = v[k - i];
                }
                else
                {
                    A(k, p) -= v[k - i] * w[p - i];
                }
            }
        }
        d[i] = beta;
    }
}

void Solver::rotate(Matrix<Real> &A, Matrix<Real> &Q, int p, int q)
{
    int n = A.row();
    Real cs[2];
    if (A(p, q) == 0)
    {
        cs[0] = 1;
        cs[1] = 0;
    }
    else
    {
        // t*t + 2*tau*t - 1 = 0
        Real tau = (A(q, q) - A(p, p)) / (2 * A(p, q));
        Real t = SIGN(tau);
        t /= (fabs(tau) + sqrt(1 + tau * tau));
        cs[0] = 1 / (sqrt(1 + t * t));
        cs[1] = t * cs[0];
    }
    // JT * A
    for (int j = 0; j < n; j++)
    {
        Real tmp;
        tmp = A(p, j);
        A(p, j) = tmp * cs[0] - A(q, j) * cs[1];
        A(q, j) = cs[1] * tmp + A(q, j) * cs[0];
    }
    // JT * A * J, note: here J is not a symmetical matrix
    // Q * J
    for (int j = 0; j < n; j++)
    {
        Real tmp;
        tmp = A(j, p);
        A(j, p) = tmp * cs[0] - A(j, q) * cs[1];
        A(j, q) = cs[1] * tmp + A(j, q) * cs[0];

        tmp = Q(j, p);
        Q(j, p) = tmp * cs[0] - Q(j, q) * cs[1];
        Q(j, q) = cs[1] * tmp + Q(j, q) * cs[0];
    }
}

void Solver::HAH(Matrix<Real> &A, int startOfRow, int endOfRow, int startOfCol, int endOfCol, Vector<Real> &v)
{
    // get houseHolder, beta, v
    // here householder will "not" change x, so we record the old length
    Real beta = v[0];
    v[0] = 1;
    Vector<Real> w(endOfCol - startOfCol);

    // caculate HA
    for (int j = startOfCol; j < endOfCol; j++)
    {
        Real sum = 0;
        // get w, (I - beta * v * vT) * A = A - v * wT
        // w = beta * AT * v
        for (int k = startOfRow; k < endOfRow; k++)
        {
            sum += A(k, j) * v[k - startOfRow];
        }
        w[j - startOfCol] = beta * sum;
    }
    // get A - v *wT
    for (int j = startOfRow; j < endOfRow; j++)
    {
        for (int k = startOfCol; k < endOfCol; k++)
        {
            A(j, k) -= v[j - startOfRow] * w[k - startOfCol];
        }
    }
    // caculate HAH, here we caculate H * (HA)T, so the form ti the same
    for (int j = startOfCol; j < endOfCol; j++)
    {
        Real sum = 0;
        for (int k = startOfRow; k < endOfRow; k++)
        {
            sum += A(j, k) * v[k - startOfRow];
        }
        w[j - startOfCol] = beta * sum;
    }
    // get A - v *wT
    for (int j = startOfRow; j < endOfRow; j++)
    {
        for (int k = startOfCol; k < endOfCol; k++)
        {
            A(k, j) -= v[j - startOfRow] * w[k - startOfCol];
        }
    }
}

void Solver::HAH(Matrix<Real> &A, int startOfRow, int endOfRow, Vector<Real> &v)
{
    // HAH = A - v *wT - w * vT
    Real beta = v[0];
    v[0] = 1;
    Vector<Real> u(endOfRow - startOfRow), w(endOfRow - startOfRow);

    // u = beta * A * v
    for (int i = startOfRow; i < endOfRow; i++)
    {
        Real sum = 0;
        for (int j = startOfRow; j < endOfRow; j++)
        {
            sum += A(i, j) * v[j - startOfRow];
        }
        u[i - startOfRow] = beta * sum;
    }
    // w = u - 1/2 * beta * (vT *u) * v
    w = u - v * (v * u) * beta * 0.5;
    for (int i = startOfRow; i < endOfRow; i++)
    {
        for (int j = startOfRow; j < endOfRow; j++)
        {
            A(i, j) -= v[i - startOfRow] * w[j - startOfRow] + v[j - startOfRow] * w[i - startOfRow];
        }
    }
}

void Solver::Hessenberg(Matrix<Real> &A, Matrix<Real> &Q, bool ifSymmetric)
{
    int n = A.row();
    for (int i = 0; i < n - 2; i++)
    {
        Vector<Real> x = A(i).range(i + 1, n), v(n - i - 1);
        Real beta = Householder(x, v);
        v[0] = beta;
        ifSymmetric ? HAH(A, i + 1, n, v) : HAH(A, i + 1, n, i, n, v);
        Q.insert(i + 1, i, v);
    }
}

void Solver::Real_QR(Matrix<Real> &A, int l, int m)
{
    int n = A.row();
    int _m = n - 1;
    Real s = A(_m - 1, _m - 1) + A(_m, _m);
    Real t = A(_m - 1, _m - 1) * A(_m, _m) - A(_m - 1, _m) * A(_m, _m - 1);
    Vector<Real> x(3);
    x[0] = A(0, 0) * A(0, 0) + A(0, 1) * A(1, 0) - s * A(0, 0) + t;
    x[1] = A(1, 0) * (A(0, 0) + A(1, 1) - s);
    x[2] = A(1, 0) * A(2, 1);
    // here we ignore the position that n<3!!!!!!!!!!!!!
    // get P0 * H * P0

    Vector<Real> v(3);
    Real beta;
    beta = Householder(x, v);
    v[0] = beta;
    HAH(A, l, l + 3, l, n, v);

    x[0] = A(l + 1, l);
    x[1] = A(l + 2, l);
    if (l + 3 < n - m)
    {
        x[2] = A(l + 3, l);
        for (int i = l + 1; i < n - m - 2; i++)
        {
            // householder
            beta = Householder(x, v);
            v[0] = beta;
            HAH(A, i, i + 3, i - 1, n, v);

            x[0] = A(i + 1, i);
            x[1] = A(i + 2, i);
            if (i < n - m - 3)
            {
                x[2] = A(i + 3, i);
            }
        }
    }
    Vector<Real> y({x[0], x[1]});
    Vector<Real> v2(2);
    beta = Householder(y, v2);
    v2[0] = beta;
    HAH(A, n - m - 2, n - m, n - m - 3, n, v2);
}

void Solver::Wilkinson_QR(Matrix<Real> &A, int l, int m)
{
    int n = A.row();
    int last = n - m - 1;
    Real d = (A(last - 1, last - 1) - A(last, last)) / 2;
    int sign = SIGN(d);
    Real u = A(last, last) - A(last, last - 1) * A(last, last - 1) / (d + sign * sqrt(d * d + A(last, last - 1) * A(last, last - 1)));
    Vector<Real> x({A(l, l) - u, A(l + 1, l)});
    Vector<Real> cs(2);
    for (int i = l; i < last; i++)
    {
        cs = Givens(x);
        // GT
        for (int j = 0; j < n; j++)
        {
            Real tmp = 0;
            tmp = A(i, j);
            A(i, j) = tmp * cs[0] + A(i + 1, j) * cs[1];
            A(i + 1, j) = -1 * cs[1] * tmp + A(i + 1, j) * cs[0];
        }
        // GTG^T, note: here G is not a symmetical matrix
        for (int j = 0; j < n; j++)
        {
            Real tmp = 0;
            tmp = A(j, i);
            A(j, i) = tmp * cs[0] + A(j, i + 1) * cs[1];
            A(j, i + 1) = -1 * cs[1] * tmp + A(j, i + 1) * cs[0];
        }
        if (i < last - 1)
        {
            x[0] = A(i + 1, i);
            x[1] = A(i + 2, i);
        }
    }
}

// ===================================================================================================

Vector<Real> Solver::solve_upper(Matrix<Real> &A, Vector<Real> &b, bool unit)
{
    int n = b.size();
    Vector<Real> x(n);
    Real sum;
    for (int i = n - 1; i >= 0; i--)
    {
        sum = 0;
        for (int j = i + 1; j < n; j++)
        {
            sum += A(i, j) * x[j];
        }
        x[i] = (b[i] - sum) / (unit ? 1 : A(i, i));
    }
    return x;
}

Vector<Real> Solver::solve_lower(Matrix<Real> &A, Vector<Real> &b, bool unit)
{
    int n = b.size();
    Vector<Real> x(n);
    Real sum;
    x[0] = b[0] / (unit ? 1 : A(0, 0));
    for (int i = 1; i < n; i++)
    {
        sum = 0;
        for (int j = 0; j < i; j++)
        {
            sum += A(i, j) * x[j];
        }
        x[i] = (b[i] - sum) / (unit ? 1 : A(i, i));
    }
    return x;
}

Vector<Real> Solver::solve(Matrix<Real> &A, Vector<Real> &b, bool choose)
{
    int n = b.size();
    Vector<Real> x(n);
    if (choose)
    {
        Perm perm(n);
        LU(A, perm);
        b = perm * b;
    }
    else
    {
        LU(A);
    }
    // 单位下三角阵
    x = solve_lower(A, b, true);
    x = solve_upper(A, x);
    return x;
}

Vector<Real> Solver::cholesky(Matrix<Real> &A, Vector<Real> &b, bool improve)
{
    int n = b.size();
    Vector<Real> x(n);
    if (improve)
    {
        Cholesky_D(A);
        // 将对角元乘到上三角部分
        for (int i = 0; i < n; i++)
        {
            for (int j = i + 1; j < n; j++)
            {
                A(i, j) *= A(i, i);
            }
        }
        x = solve_lower(A, b, true);
        x = solve_upper(A, x);
    }
    else
    {
        Cholesky(A);
        x = solve_lower(A, b);
        x = solve_upper(A, x);
    }
    return x;
}

Vector<Real> Solver::qr(Matrix<Real> &A, Vector<Real> &b)
{
    int row = A.row(), col = A.col();
    Vector<Real> d(col);
    QR(A, d);

    // Householder 变换作用于 b
    // Hb = b - beta * v * v^T * b = b - (beta * v^T * b) * v
    for (int i = 0; i < col; i++)
    {
        Real sum = b[i];
        for (int j = i + 1; j < row; j++)
        {
            sum += A(j, i) * b[j];
        }
        sum *= d[i], b[i] -= sum;
        for (int j = i + 1; j < row; j++)
        {
            b[j] -= A(j, i) * sum;
        }
    }
    // 求解上半部分的线性方程组
    Vector<Real> b1(b.range(0, col));
    Vector<Real> x(solve_upper(A, b1));
    return x;
}

Vector<Real> Solver::Jacobi(Matrix<Real> &A, Vector<Real> &b)
{
    int n = A.order();
    // x = D1(L+U) * x + D1 * g
    Vector<Real> x(n, 1), y(n); // let x be a all one vector
    Real sum, dx;
    do
    {
        dx = 0;
        // 分步迭代过程
        for (int i = 0; i < n; i++)
        {
            sum = 0;
            for (int j = 0; j < n; j++)
            {
                if (i != j)
                {
                    sum -= A(i, j) * x[j];
                }
            }
            y[i] = (sum + b[i]) / A(i, i);
        }
        dx = Vec::norm(y - x, 0);
        x = y;
    } while (fabs(dx) >= ACCURACY);
    return x;
}

Vector<Real> Solver::G_S(Matrix<Real> &A, Vector<Real> &b)
{
    int n = A.order();
    Real sum, dx;
    Vector<Real> x(n, 1);

    do
    {
        dx = 0;
        // 分部迭代过程
        for (int i = 0; i < n; i++)
        {
            sum = 0;
            for (int j = 0; j < n; j++)
            {
                if (j != i)
                {
                    sum -= A(i, j) * x[j];
                }
            }
            Real tmp = x[i];
            x[i] = (sum + b[i]) / A(i, i);
            dx = MAX(fabs(tmp - x[i]), dx);
        }
    } while (dx >= ACCURACY);
    return x;
}

Vector<Real> Solver::SOR(Matrix<Real> &A, Vector<Real> &b, Real w)
{
    // x = Lw * x + w(D-wL)1 * b
    int n = A.order();
    Vector<Real> x(n, 1);
    Real sum, dx;

    do
    {
        dx = 0;
        // 分部迭代过程
        for (int i = 0; i < n; i++)
        {
            sum = 0;
            for (int j = 0; j < n; j++)
            {
                if (j != i)
                {
                    sum -= A(i, j) * x[j];
                }
            }
            Real tmp = x[i];
            x[i] = (1 - w) * x[i] + w * (sum + b[i]) / A(i, i);
            dx = MAX(fabs(tmp - x[i]), dx);
        }
    } while (dx >= ACCURACY);
    return x;
}

Real Solver::kappa(Matrix<Real> &A)
{
    int n = A.order();
    Real A_K = Mat::norm(A, 0); // A's condition number
    Real A1_K = 0;              // A-1's condition number
    Matrix<Real> AT(Mat::transpose(A));
    Matrix<Real> _A(A);
    Vector<Real> x(n, 1.0 / n), w(n), v(n), z(n);

    // 分别对 AT 和 A 做分解
    LU(AT);
    LU(A);

    // 判断是否终止
    bool yes = false;
    int j = 0;

    while (!yes)
    {
        // get w
        w = solve_lower(AT, x);
        w = solve_upper(AT, w);
        v = static_cast<Vector<Real>>(Vec::sgn(w));
        // get z
        z = solve_lower(A, v);
        z = solve_upper(A, z);
        // compare infinite z and zT * x
        if (Vec::norm(z, 0) <= Vec::dot(z, x))
            break;
        x = Vector<Real>::unit(n, Vec::max_index_of(z));
    }
    // A1_K = one w
    A1_K = Vec::norm(w, 1);
    return A_K * A1_K;
}

Real Solver::caculate_analysis(Matrix<Real> &A, Vector<Real> &x, Vector<Real> &b)
{
    int n = A.order();
    Vector<Real> r(A * x - b);
    Real kr, kb, K;
    kr = Vec::norm(r, 0);
    kb = Vec::norm(b, 0);
    K = kappa(A);
    return K * kr / kb;
}

Vector<Real> Solver::cg(Matrix<Real> &A, Vector<Real> &b)
{
    int row = A.row(), column = A.col(), n = ITERATION;
    Vector<Real> x(row, 1), p(row), r(row);
    p = b - A * x, r = p;                     // get p_0 = r_0
    Real rr = Vec::dot(r, r), nrr = rr, beta; // get r0 * r0, next r0 * r0
    Real alpha = rr / Vec::dot(p, A * p);     // get alpha_0 = r0T * r0 / p0T * A * p0
    x = p * alpha + x;                        // get x_0

    while (sqrt(rr) > ACCURACY && n > 0)
    {
        r = r - A * p * alpha;           // get r = rk-1 - alphak-1 * A * pk-1
        nrr = Vec::dot(r, r);            // get next r * r, nrr = r * r
        beta = nrr / rr;                 // get beta = rkT * rk / rk-1T * rk-1
        rr = nrr;                        // get p = rk + betak-1 * pk-1
        p = r + p * beta;                // get alpha
        alpha = rr / Vec::dot(p, A * p); // get x
        x = p * alpha + x;
        n--;
    }
    return x;
}

Real Solver::power(Matrix<Real> &A, Real mu)
{
    int n = A.order();
    Vector<Real> x(n, 1);
    Matrix<Real> _A(n, n);
    _A = A - x * mu;

    Real dp = 1, p, q = 0;
    Vector<Real> y(n);
    while (dp > ACCURACY)
    {
        y = _A * x;
        p = y[Vec::max_index_of(y)];
        x = y / p;
        dp = fabs(p - q);
        q = p;
    }
    return p + mu;
}

Vector<Real> Solver::inverse_power(Matrix<Real> &A, Real lambda)
{
    int n = A.order();
    Vector<Real> e(n, 1);
    Matrix<Real> _A(n, n);
    _A = A - e * lambda;
    // z_0 = P^T * L * e, U * v_1 = e
    // z = PermOp::inverse(p) * (lower(_A) + z) * z;

    Perm p(n);
    LU(_A, p);
    e = solve_upper(_A, e);
    return Vec::normalize(e, 0);
}

int Solver::implicit_qr(Matrix<Real> &A, bool ifSymmetric)
{
    int l, m, n = A.order(), count = 0;
    Matrix<Real> Q(n, n);
    Hessenberg(A, Q, ifSymmetric);
    while (count < ITERATION)
    {
        // Zero
        for (int i = 1; i < n; i++)
        {
            if (fabs(A(i, i - 1)) <= ((fabs(A(i - 1, i - 1)) + fabs(A(i, i))) * ACCURACY))
            {
                A(i, i - 1) = 0;
                if (ifSymmetric)
                {
                    A(i - 1, i) = 0;
                }
            }
        }
        for (int i = 2; i < n; i++)
        {
            for (int j = 0; j < i - 1; j++)
            {
                A(i, j) = 0;
                if (ifSymmetric)
                {
                    A(j, i) = 0;
                }
            }
        }
        // divide to three part
        m = 0;
        Real last = A(n - 1, n - 2);
        for (int i = n - 2; i > 0; i--)
        {
            if (A(i, i - 1) == 0 || last == 0)
            {
                m++;
            }
            else
            {
                break;
            }
            last = A(i, i - 1);
            if (i - 1 == 0)
            {
                m = n;
            }
        }
        if (m == n)
        {
            break;
        }
        else
        {
            l = 0;
            for (int i = n - m - 1; i > 0; i--)
            {
                if (A(i, i - 1) != 0)
                {
                    l++;
                }
            }
            l++;
            l = n - m - l;
        }
        // Real QR
        if (ifSymmetric)
        {
            Wilkinson_QR(A, l, m);
        }
        else
        {
            Real_QR(A, l, m);
        }
        count++;
    }
    return count;
}

// bool Solver::eigen(Matrix<Real> &A, Matrix<Complex> &eigenM, Vector<Real> &eigenV)
// {
//     implicit_qr(A); // 约化为拟上三角阵
//     int n = A.row();
//     for (int i = 0; i < n; i++)
//     {
//         if (i < n - 1 && A(i + 1, i) != 0)
//         {
//             // 分离虚实部的反幂法
//             Complex z = algo::eigen2(A, i, i);
//             Vector<Complex> v(n);

//             // M = (A-aI)^2 + b^2 * I
//             // p_k = e_1, q_k = 0, (A-aI)e_1, be_1
//             Matrix<Real> M(n, n);
//             M = A - Vector<Real>::unit(n, 1) * z.getReal();
//             Vector<Real> p(M(0)), q(n);
//             q[0] = z.getImage();
//             M = M * M + Vector<Real>::unit(n, 1) * z.getImage() * z.getImage();

//             Matrix<Real> sM = M;
//             p = solve(sM, p, true);
//             sM = M;
//             q = solve(sM, q, true);

//             for (int i = 0; i < n; i++)
//             {
//                 v[i] = Complex(p[i], q[i]);
//             }

//             eigenV[i] = z.getReal();
//             eigenV[i + 1] = z.getImage();
//             eigenM.insert(0, i, v);
//             i++;
//         }
//         else
//         {
//             eigenV[i] = A(i, i);
//             Vector<Real> tmp = inverse_power(A, A(i, i));
//             Vector<Complex> v(tmp);
//             eigenM.insert(0, i, v);
//         }
//     }
//     return true;
// }

Matrix<Real> Solver::pass_Jacobi(Matrix<Real> &A, Real sigma)
{
    int n = A.order();
    Matrix<Real> Q(Matrix<Real>::unit(n));
    if (sigma < n)
    {
        return Q;
    }
    Real delta = Mat::norm(A, 'e');
    bool check;
    int count = 0;
    while (delta > ACCURACY && count < ITERATION)
    {
        check = true;
        for (int i = 0; i < n; i++)
        {
            for (int j = i + 1; j < n; j++)
            {
                if (fabs(A(i, j)) > delta)
                {
                    rotate(A, Q, i, j);
                    check = false;
                }
            }
        }
        if (check)
        {
            delta /= sigma;
        }
        count++;
    }
    return Q;
}

Vector<Real> Solver::bisection(Matrix<Real> &A)
{
    int n = A.order();
    Vector<Real> lambda(n), alpha(n), beta(n);
    Real u = Mat::norm(A, 0);
    Real l = -1 * u;
    for (int i = 0; i < n; i++)
    {
        alpha[i] = A(i, i);
        if (i > 0)
        {
            beta[i] = A(i, i - 1);
        }
    }
    for (int i = 0; i < n; i++)
    {
        Real _u = u, _l = l;
        Real nowE = u - l, r = (u + l) / 2;
        while (nowE > ACCURACY)
        {
            // compare i+1 and sign reverse number
            int s = 0;
            Real q = alpha[0] - r;
            for (int j = 0; j < n; j++)
            {
                if (q < 0)
                {
                    s++;
                }
                if (j < n - 1)
                {
                    if (q == 0)
                    {
                        // replace q with a little positive number
                        q = fabs(beta[j + 1]) * ACCURACY;
                    }
                    q = alpha[j + 1] - r - beta[j + 1] * beta[j + 1] / q;
                }
            }
            if (s >= (i + 1))
            {
                _u = r;
            }
            else
            {
                _l = r;
            }
            r = (_u + _l) / 2;
            nowE = _u - _l;
        }
        lambda[i] = r;
    }
    return lambda;
}